%% constrained Gauss-Newton
%clear all;
close all;
clc;

% Defining constants
n_it = 200;
quad_cent = [50; 0];
gamma = 0.1;
beta = 0.8;
tol = 1E-1;

Restlength = 0.8;
D = 3000;
m = 80;
g = 9.81;

infe_gn = [];
% Initial value
N=6;
tensor = zeros(N,N,3);
for i=1:N,
    tensor(i,:,1)=linspace(-N/2,N/2,N);
    tensor(:,i,2)=linspace(-N/2,N/2,N);
end
tensor(:,:,3)=1;
x=tensor2array(tensor);

% phi = linspace(0,2*pi, 100);
% y_c = sin(phi) * 1;
% x_c = cos(phi) * 1;
% 
figure()
% plot(0, 0, 'O', x_c, y_c,'-');
% hold on;
% axis equal;
% plot( x(1), x(2), 'X')
% % plot(quad_cent(1), quad_cent(2), 'xr');
% drawnow

fprintf('It.\t\t||∇L||\t||g||\t||Δx||\tt\n');

% Linearizing objective
[F_val J] = finite_difference(x, @cloth,Restlength,D,m,g);
% Linearizing constraint
[g_val Jg ] = finite_difference(x, @cloth_eqconstr,Restlength,D,m,g);

for k = 1 : n_it
   % Building up QP
   H = J' * J;
   c = J' * F_val;
   
   % This is needed to make the QP solver silent
   opt = optimset('Display', 'off', 'LargeScale', 'off');
   % We solve the QP and obtain search direction "dx" and dual variables
   % "lambda"
   [dx fval flag output lambda] = quadprog(H, c, [], [], Jg, -g_val, [], [], [], opt);
   % We calculate the "sigma" of the merit function
   sigma = norm(lambda.eqlin, inf);
   lambda = lambda.eqlin;
   % Evaluate merit function in the actual iterate
   T_act = F_val' * F_val / 2 + sigma * norm(g_val, inf);
   % Calculate the directional derivative
   T_dir = (J' * F_val)' * dx - sigma * norm(g_val, 1);
   % Default value for line-search
   t = 1.0;
   % Preparing the first candidate
   x_new = x + t * dx;
   F_new = cloth(x_new,Restlength,D,m,g);
   g_new = cloth_eqconstr(x_new,Restlength,D,m,g);
   % Evaluate merit function in the actual iterate
   T_new = (F_new' * F_new) / 2 + sigma * norm(g_new, inf);
   % Checking Armijo's condition (on the merit function)
   while (T_new > T_act + gamma * t * T_dir)
      t = beta * t;
      x_new = x + t * dx;
      F_new = cloth(x_new,Restlength,D,m,g);
      g_new = cloth_eqconstr(x_new,Restlength,D,m,g);
      T_new = (F_new' * F_new) / 2 + lambda * norm(g_new, inf) ;
   end
   % By now the candidate is accepted by Armijo's condition
   x = x_new;
   [F_val J] = finite_difference(x, @cloth,Restlength,D,m,g);
   [g_val Jg ] = finite_difference(x, @cloth_eqconstr,Restlength,D,m,g);
   % Note that the original problem's Lagr. multiplers are different in a
   % "-" sign.
   lag_grad = J' * F_val + Jg' * lambda;
   fprintf('%d\t %f\t %f\t %f\t %f\n', k, norm(lag_grad), norm(g_val), norm(dx), t)
   % Checking stopping criterion
%    if norm([lag_grad; g_val]) < tol
%       fprintf('Convergence achieved\n')
%       break
%    end
%    infe_gn = [infe_gn; norm([lag_grad; g_val])];
   plotcloth(x,Restlength);
   drawnow;

end